global bound l para result npout
l=[1,1,1,1,0.5];
bound=[ -pi     pi;% qm1
        -pi     pi;% qm2
        -pi     pi;% qm3
        -pi     pi;% qm4
        -pi     pi;% qm5
       -pi/4   pi/4;% vqm1
       -pi/4   pi/4;% vqm2
       -pi/4   pi/4;% vqm3
       -pi/4   pi/4;% vqm4
       -pi/4   pi/4;% vqm5
        -pi     pi;% phis1
        -pi     pi;% phis2
        -pi     pi;% phis3
        -pi     pi;% phif1
        -pi     pi;% phif2
        -pi     pi;% phif3
        0       8;% t1
        0       8];%t2
npout=40;
xs=1;ys=0;
xf=1;yf=4;

para=[xs ys xf yf];

% cacu_result(5,para,100,120,@fitnessf);
% fmin=BSO_fun(100, 100,-1,1,18,@fitnessf);
% [x1,x2,x3,it]=trajectory_planningf_GA(para,500,200,@fitnessf)
% [x1,x2,x4]=trajectory_planningf_PSO(para,@fitnessf,@trajplot5,@paraplot5)
% [x1,x2,x4]=trajectory_planningf_BSO(para,100,200,@fitnessf,@trajplot5,@paraplot5)
% [x21,x22,x23,x24]=trajectory_planningf_xBSO(para,@recor4,@trajplot4,@paraplot4)
% [x1,x2,x4]=trajectory_planningf_BAS(para,@fitnessf);
iter_store=zeros(300,2);
for i=1:300
[x1,x2,x3,it]=trajectory_planningf_GA(para,i+10,200,@fitnessf);
iter_store(i,1)=it;
iter_store(i,2)=x2;
end

function [aa,fit_v] = fitnessf(pop,para)
global l bound npout
[pops,~]=size(pop);
aa=zeros(pops,1);
fit_v=zeros(pops,4);
for i=1:pops
    fit=0;
    a_v=[ 2;  %ft 2
          1.8;  %fq 1.8
          2;  %fdis 2
          1]; %time 1
%     bsol=(pop(i,:)+1)./2.*(bound(:,2)-bound(:,1))'+bound(:,1)';
    bsol=pop(i,:);
    time=bsol(17)+bsol(18);
    [m_s,confs]=invkin5(para(1),para(2),bsol(11),bsol(12),bsol(13),l);
    [m_f,conff]=invkin5(para(3),para(4),bsol(14),bsol(15),bsol(16),l);
%     bsol(11:13)=phis;
%     bsol(14:16)=phif;
    if strcmp(m_s,'no-redundancy') || strcmp(m_f,'no-redundancy')
        if strcmp(m_s,'no-redundancy')
            fit=fit+(confs(1))*500;
        end
        if strcmp(m_f,'no-redundancy')
            fit=fit+(conff(1))*500;
        end
        aa(i)=1/fit;
        fit_v(i,:)=[0 0 0 0];
        continue;
    end
    cond=[confs,conff];
    chrom=[bsol(1:10),bsol(17:18)];
    kk=trajt(cond,chrom,npout);
%     tt=torque5(kk);
%     ft=ftorque5(tt);
    ft=ftorque5(kk(11:15,:));
    poo=kk(1:5,:);
    fq=sum(sum(abs(diff(poo'))));
    pos=forkin(poo,l);
    xx=pos(1,:);yy=pos(2,:);
    x=diff(xx);
    y=diff(yy);
    dis=sqrt(x.^2+y.^2);
    fdis=sum(dis);
    fit_v(i,:)=[ft,fq,fdis,time];
    fit=fit_v(i,:)*a_v;
    aa(i)=1/fit;
end
end

function trajplot5(para,bsol,fit,i,n)
global l kk npout
    [~,confs]=invkin5(para(1),para(2),bsol(11),bsol(12),bsol(13),l);
    [~,conff]=invkin5(para(3),para(4),bsol(14),bsol(15),bsol(16),l);
    qs=confs;
    qm=bsol(1:5);
    qg=conff;
    
    [xt,yt]=angls2links([qs' qm' qg'],l);
    cond=[confs,conff];
    chrom=[bsol(1:10),bsol(17:18)];
    kk=trajt(cond,chrom,npout);
    pcart=forkin(kk(1:5,:),l);
    px=pcart(1,:);py=pcart(2,:);
    plot(px,py)
    hold on
    axis([-6 6 -6 6])
    text(3.5,4,['gen. No.',num2str(i)]);
    text(3.5,3.5,['fit=',num2str(fit)]);
    text(3.5,3,['time=',num2str(bsol(17)+bsol(18))]);
    plot(xt',yt')
    hold on
    plot(0,0,'ko')
    for j=1:4
        plot(xt(:,size(xt,2)/5*j),yt(:,size(xt,2)/5*j),'bo')
    end
    hold off
end

function paraplot5(n,maxf,bsols,rec)
global l kk para
%     trec=rec(1,:);
%     qrec=rec(2,:);
%     drec=rec(3,:);
%     torrec=rec(4,:);
    
    figure,manimove_anime(kk(1:5,:),l);

    e=[1:n];
    [aa,fit_v]=fitnessf(bsols,para);
    fit_v(n,:)
    figure,plot(e,1./maxf)
    xlabel('generation')
    ylabel('min. fitness')
    hold on,
    plot(e,fit_v'*2)
end